// task_server_node.hpp
#ifndef TASK_SERVER_NODE_HPP_
#define TASK_SERVER_NODE_HPP_

#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <rclcpp/client.hpp>
#include <rms_pkg/action/task_execution.hpp>
#include <rms_pkg/srv/robot_status.hpp>
#include <memory>
#include <thread>
#include <mutex>
#include <QObject>
#include <QTimer>
#include <QString>
#include <map>
#include <string>
#include <unordered_map>
#include "../include/db/database_manager.h"
#include "../include/seer/SCStatusTcp.h"
#include "../include/seer/RequestManager.h"
#include "../include/global_logger.h"

// 前向声明
class SCStatusTcp;
class RequestManager;
class DatabaseManager;

using TaskExecution = rms_pkg::action::TaskExecution;
using GoalHandle = rclcpp_action::ServerGoalHandle<TaskExecution>;

// 任务上下文结构
struct TaskContext {
    std::string task_id;
    std::shared_ptr<const TaskExecution::Goal> goal;
    std::shared_ptr<GoalHandle> goal_handle;
    rclcpp::Time start_time;
    rclcpp::Time end_time;
};

class TaskServerNode : public QObject, public rclcpp::Node
{
    Q_OBJECT

public:
    explicit TaskServerNode();

private:
    rclcpp_action::Server<TaskExecution>::SharedPtr action_server_;
    rclcpp::Client<rms_pkg::srv::RobotStatus>::SharedPtr m_robotStatusClient;

    // SRC2000控制器连接相关
    SCStatusTcp* m_naviSocket;  // SRC2000控制器导航socket
    SCStatusTcp* m_confSocket;  // SRC2000控制器配置socket(抢占控制权)
    SCStatusTcp* m_otherSocket; // SRC2000控制器其他socket(设置DO控制喷药继电器)
    SCStatusTcp* m_yoloSocket;  // YOLO图像处理子任务socket
    RequestManager* m_requestManager;
    
    // 重连定时器
    QTimer* m_reconnectTimer;
    QTimer* m_healthCheckTimer;  // 健康检查定时器
    
    // 连接参数
    QString m_seerIP;
    QString m_yoloIP;  // YOLO任务IP地址
    QString m_taskAbsolutePath;   // 保存当前任务绝对路径
    int m_naviPort;
    int m_confPort;
    int m_otherPort;
    int m_yoloPort;  // YOLO任务端口

    // 喷药控制SRC2000 GPIO序号参数
    int spraying_gpio_id_;

    // 连接状态
    bool m_wasConnected;  // 记录上一次连接状态
    
    // 为每个套接字添加独立的重连计数器
    int m_naviReconnectAttempts = 0;
    int m_confReconnectAttempts = 0;
    int m_otherReconnectAttempts = 0;
    int m_yoloReconnectAttempts = 0;
    
    // 套接字连接状态标志
    bool m_naviConnected = false;
    bool m_confConnected = false;
    bool m_otherConnected = false;
    bool m_yoloConnected = false;
    
    // 任务管理
    std::map<std::string, TaskContext> m_taskContexts;
    std::map<std::string, std::shared_ptr<QTimer>> m_navigationTimers;
    QString m_previousTarget;  // 跟踪上一个目标点
    bool m_isSpraying  = false; // 跟踪当前喷药状态
    std::unordered_set<std::string> m_sprayedLobes; // 已喷药的陇区间

    std::string m_acquirerControlName = "RMSClient";    //申请控制权NickName
    bool hasControlAuthority = false;                   // 是否拥有控制权

    // 数据库管理器
    std::shared_ptr<DatabaseManager> dbManager_;

    //保存当前处理的任务信息
    struct CurrentTaskContext {
        std::shared_ptr<const TaskExecution::Goal> goal;
        std::shared_ptr<GoalHandle> goal_handle;
        rclcpp::Time start_time;
        QByteArray json_data;
    };
    std::unique_ptr<CurrentTaskContext> m_currentTaskContext;               //当前任务上下文数据

    rclcpp_action::GoalResponse handle_goal(
        const rclcpp_action::GoalUUID &uuid,
        std::shared_ptr<const TaskExecution::Goal> goal);                   // 接受action目标请求

    rclcpp_action::CancelResponse handle_cancel(
        const std::shared_ptr<GoalHandle> goal_handle);                     // 拒绝任务

    void handle_accepted(const std::shared_ptr<GoalHandle> goal_handle);    // 接受任务

    void execute_task(const std::shared_ptr<GoalHandle> goal_handle);       // 任务执行函数
    
    // 任务执行和监控相关函数
    void startNavigationMonitoring(const std::string& task_id); // 启动导航监控
    void queryNavigationStatus(const std::string& task_id, std::shared_ptr<QTimer> timer);  // 启动导航监控
    int getTaskStatus(const QString& state);    // 获取任务状态
    void exportSensorData(const std::string& task_id, rclcpp::Time start_time, rclcpp::Time end_time);  // 导出传感器数据到csv文件
    void saveTaskResultToDatabase(const std::string& task_id, int taskStatus, 
                                  rclcpp::Time start_time, rclcpp::Time end_time);                      // 保存任务结果到数据库
    void handleRobotStatusResponse(const std::string& task_id, std::shared_ptr<QTimer> timer,
                                  rclcpp::Client<rms_pkg::srv::RobotStatus>::SharedFuture future);      // 处理机器人状态响应

    // 喷药子任务相关函数
    void handleSprayingTask(const std::string& task_id,
                           const rms_pkg::srv::RobotStatus::Response::SharedPtr& robotStatus,
                           const TaskContext& context);                                               // 处理喷药任务
    bool isCurrentTargetInLobe(const QString& currentTarget, const std::string& lobeData);            // 检查当前位置是否在陇区间内
    void sendSprayingCommand(bool enable);                                                            // 发送喷药控制命令
    std::pair<bool, QString> getLobeForPath(const QString& lastStation, const QString& currentTarget, const std::string& lobeData);  // 获取当前站点所属的陇区间

    // YOLO子任务相关函数
    void sendTaskStartToYolo(const std::string& task_id, 
                           const std::shared_ptr<const TaskExecution::Goal> goal);                    // 发送任务开始数据给YOLO子任务
    void sendTaskEndToYolo(const std::string& task_id);                                               // 向YOLO子任务发送任务结束通知
    void sendLocationUpdateToYolo(const std::string& task_id,
                                 const QString& currentLocation,
                                 const QString& targetLocation);                                      // 向YOLO子任务发送位置更新信息
    // 任务结束后处理特定类型任务的清理工作
    void handleTaskTypeSpecificCleanup(const std::shared_ptr<const TaskExecution::Goal> goal);


    // 命令处理函数
    void createTaskDirectory(const std::string& task_id);               //创建任务目录
    void acquireControlAuthority(const QString& nickName, int cmdId);   //抢占控制权
    void sendPauseCommand(const std::shared_ptr<const TaskExecution::Goal> goal);   //暂停任务
    void sendResumeCommand(const std::shared_ptr<const TaskExecution::Goal> goal);  //恢复任务
    void sendCancelCommand(const std::shared_ptr<const TaskExecution::Goal> goal);  //取消任务
    void sendNavigationCommand(const std::shared_ptr<const TaskExecution::Goal> goal, const QByteArray& jsonData);  //导航任务
    void handleRobotResponseForTask(int cmdId, const QByteArray& response);         //处理机器人返回的响应

    // 连接管理
    void initializeConnections();
    void connectToRobot();
    bool isAllConnected() const;            // 检查全部Socket连接状态
    bool isNaviConnected() const;           // 检查导航套接字连接状态
    bool isConfConnected() const;           // 检查配置套接字连接状态
    bool isOtherConnected() const;          // 检查其他任务套接字连接状态
    bool isYoloConnected() const;           // 检查YOLO任务套接字连接状态
    bool areAllCriticalConnected() const;   // 检查关键套接字连接状态(导航和配置)
    void handleSocketStateChanged(QAbstractSocket::SocketState state);
    void handleSocketError(QAbstractSocket::SocketError error);
    
    // 重连机制
    void scheduleReconnect();
    void resetReconnectAttempts();
    
    // 健康检查
    void healthCheck();
    void updateConnectionStatus();
    
    // 命令执行
    void executeTaskCommand(const std::shared_ptr<const TaskExecution::Goal> goal,const QByteArray& jsonData,
        const std::shared_ptr<GoalHandle> goal_handle,
        const rclcpp::Time& start_time);

private slots:
    void reconnectTimerTimeout();
    void healthCheckTimeout();  // 健康检查槽函数
    // 用于处理控制权获取结果
    void onControlAuthorityAcquired(bool success);
    // 用于处理任务命令发送结果
    void onTaskCommandSent(const std::string& task_id, bool success, const std::string& message);

signals:
    // 控制权获取完成信号
    void controlAuthorityAcquired(bool success);
    // 任务命令发送完成信号
    void taskCommandSent(const std::string& task_id, bool success, const std::string& message);
};

#endif // TASK_SERVER_NODE_HPP_